perm filename TRJDSP.SAI[AL,HE] blob
sn#290096 filedate 1977-06-28 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00006 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 PROGRAM TO DISPLAY JOINT ANGLE POLYNOMIALS FROM 11TRAJ FILES
C00006 00003 ⊃ START OF EXECUTABLE CODE, READ IN THE TRAJECTORY FILE NAME AND OPEN FILE
C00010 00004 ⊃ SECTION FOR DISPLAY FUNCTION "D"
C00015 00005 ⊃ IF P COMMAND, WRITE OUT A DISK FILE CONTAINING THE DISPLAY BUFFER.
C00016 00006 ⊃ MAIN LOOP
C00035 ENDMK
C⊗;
COMMENT PROGRAM TO DISPLAY JOINT ANGLE POLYNOMIALS FROM 11TRAJ FILES;
BEGIN "TRJDSP"
DEFINE ⊃="COMMENT ",CRLF="('15&'12)",VT="'13",FUNCT="2",HEAD="3",CR="'15";
DEFINE RAD="57.29577951";
SAFE INTEGER ARRAY DISPLY[1:'3000];
STRING ANS,DANS,DISCM,RELD,NAME,COM1,COM2,COM3;
INTEGER DUM,CHAN,BRCHAR,COUNT,EOF;
SAFE REAL ARRAY TRANS[1:4,1:4],JTS[1:6];
REAL TT,TTOTAL,PT;
INTEGER CS1,CS2,N;
INTEGER MAX;
SAFE INTEGER ARRAY ANGLE[0:1000];
SAFE INTEGER ARRAY X[0:999],Y[0:999],Z[0:999],O[0:999],A[0:999],T[0:999];
INTEGER NXTSEG,JT,POG,SEG1,SEG2,STEPS,CT;
SAFE INTEGER ARRAY TIME[1:20];
INTEGER I,J,K,NSEG;
SAFE REAL ARRAY EANGLE[1:6];
SAFE REAL ARRAY COEF[1:20,1:6,1:6];
REQUIRE"DPYSUB.HDR[1,PDQ]" SOURCE_FILE;
REQUIRE"UPDATE.REL[AL,HE]" LOAD_MODULE;
REQUIRE"FAITRG[AL,HE]" LOAD_MODULE;
EXTERNAL PROCEDURE UPDATE(REAL ARRAY T,J);
INTEGER PROCEDURE NUM(INTEGER CHAN);
BEGIN
INTEGER Z;
LABEL DLP;
Z←INTIN(CHAN);
START_CODE
SETZ 1,;
MOVE 2,Z;
DLP: IDIVI 2,10;
LSHC 3,-3;
ADDI 1,3;
JUMPN 2,DLP;
LSHC 3,(1);
MOVEM 3,Z;
END;
RETURN(Z);
END;
REAL PROCEDURE GETFLT(INTEGER CHAN);
BEGIN
INTEGER NUM1,NUM2,LBYTE;
REAL Z;
LABEL DNE;
NUM1←NUM(CHAN);
NUM2←NUM(CHAN);
LBYTE←'142700000002;
START_CODE
MOVE 1,NUM1;
LSH 1,16;
MOVE 2,NUM2;
ANDI 2,'177777;
IOR 1,2;
SETZ 2,;
DPB 1,LBYTE;
LSH 1,-23;
LSHC 1,-9;
JUMPE 2,DNE;
TLO 2,'400;
JUMPGE 2,DNE;
TLZ 2,'400000;
MOVN 2,2;
DNE: MOVEM 2,Z;
END;
RETURN(Z);
END;
PROCEDURE UNSTRUCT(REAL ARRAY T,E);
BEGIN
REAL CO2;
E[1]←T[1,4];
E[2]←T[2,4];
E[3]←T[3,4];
E[5]←RAD*ATAN2(-T[3,3],CO2←SQRT(T[1,3]↑2+T[2,3]↑2));
IF CO2<0.01 THEN BEGIN
E[4]←RAD*ATAN2(T[2,2],T[1,2]);
E[6]←0;
RETURN END;
E[4]←RAD*ATAN2(T[1,3],-T[2,3]);
E[6]←RAD*ATAN2(T[3,2],-T[3,1])
END;
⊃ START OF EXECUTABLE CODE, READ IN THE TRAJECTORY FILE NAME AND OPEN FILE;
INTERNAL PROCEDURE DISPLAY(STRING MESSG);
BEGIN "DISPLAY"
INTEGER ZZ,NUMJT;
LABEL GETD,GETDNE,STILMR;
LABEL NXTFUN;
LABEL TOP,NXT;
SETFORMAT(1,0);
COUNT←72;
CHAN←1;
SETBREAK(1,'12,'15,"INS");
SETBREAK(HEAD,"ABCDEFGHIJKLMNOPQRSTUVWXYZ?"&'12&'15,NULL,"KINR");
SETBREAK(FUNCT,"ABCDEFGHIJKLMNOPQRSTUVWXYZ?0123456789",NULL,"KXNR");
TOP: OPEN(CHAN,"DSK",0,2,0,COUNT,BRCHAR,EOF);
PRINT('15&'12&"ENTER FILE NAME: ");
DO BEGIN
PRINT("*");
NAME←INCHWL;
IF EQU(NAME,"") THEN BEGIN
CLOSE(CHAN);
DONE;
END;
LOOKUP(CHAN,NAME,DUM);
IF DUM THEN OUTSTR("LOOKUP FAILURE"&CRLF);
END UNTIL ¬DUM;
⊃ READ IN THE TRAJECTORY;
ZZ←NUM(CHAN);
START_CODE
MOVE 1,ZZ;
SETZ 3,;
GETD: JFFO 1,STILMR;
JRST GETDNE;
STILMR: LSH 1,1(2);
AOJA 3,GETD;
GETDNE: MOVEM 3,NUMJT;
END;
NUM(CHAN);NUM(CHAN);
NSEG←0;
DO BEGIN
NSEG←NSEG+1;
NXTSEG←NUM(CHAN);
TIME[NSEG]←NUM(CHAN);
DUM←NUM(CHAN);DUM←NUM(CHAN);
FOR I←1 STEP 1 UNTIL NUMJT DO
FOR J←1 STEP 1 UNTIL 6 DO
COEF[NSEG,I,J]←GETFLT(CHAN);
FOR J←1 STEP 1 UNTIL NUMJT*2 DO
GETFLT(CHAN); ⊃ Ignore gravity, inertia;
END UNTIL ¬NXTSEG;
CLOSE(CHAN);
⊃ NSEG ← NSEG - 1; ⊃ Since the one with NXTSEG=0 is a dummy;
OUTSTR("NUMBER OF SEGMENTS READ = "&CVS(NSEG)&CRLF);
DANS ← "D 1 1 " & CVS(NSEG) & " 10"; ⊃ Sample display command;
⊃ READ IN THE DISPLAY FUNCTION AND JOINT;
DO BEGIN "DB"
OUTSTR("TYPE FUNCT,JT #,1ST SEG,LAST SEG, STEPS/SEC"&CRLF);
LODED(DANS&CR);
ANS←RELD←INCHWL;
SCAN(ANS,HEAD,DUM);
DISCM←SCAN(ANS,FUNCT,DUM);
⊃ CHECK IF "?";
IF EQU(DISCM,"?") THEN BEGIN
OUTSTR(CRLF&"THE POSSIBLE FUNCTIONS ARE AS FOLLOWS:"&CRLF
&" A - DISPLAY THREE EULER ANGLES"&CRLF
&" C - MODIFY COMMENT LINE"&CRLF
&" D - DISPLAY JOINT ANGLE. MODE TERMINATED BY TYPING CR"&CRLF
&" E - TERMINATE DISPLAY MODE"&CRLF
&" P - OUTPUT DISPLAY BUFFER TO DISK"&CRLF
&" X - DISPLAY X, Y, Z POSITION OF HAND ORIGIN"&CRLF
&" ? - EXPLAIN POSSIBLE FUNCTIONS"&CRLF&CRLF);
GOTO NXT;
END;
⊃ MODIFY COMMENT;
IF EQU(DISCM,"C") THEN BEGIN
OUTSTR("TYPE IN A NEW COMMENT"&CRLF);
LODED(COM3&CR);
COM3←INCHWL & " "; ⊃ Pad, since else oft truncated;
GOTO NXT;
END;
⊃ SECTION FOR DISPLAY FUNCTION "D";
IF EQU(DISCM,"D") THEN CS1←CS2←0
ELSE IF EQU(DISCM,"X") THEN CS1←CS2←1
ELSE IF EQU(DISCM,"A") THEN BEGIN CS1←1;CS2←2;END
ELSE GO TO NXTFUN;
BEGIN
JT←INTSCAN(ANS,DUM);
IF (JT<1)∨(JT>NUMJT) THEN BEGIN
OUTSTR("ILLEGAL JOINT SPECIFICATION"&CRLF);
GOTO NXT;
END;
SEG1←INTSCAN(ANS,DUM);SEG2←INTSCAN(ANS,DUM);
STEPS←INTSCAN(ANS,DUM);
IF (SEG1<1)∨(SEG2>NSEG) THEN BEGIN
OUTSTR("ILLEGAL SEGMENT SPECIFIED"&CRLF);
GOTO NXT;
END;
IF (STEPS≤0)∨(STEPS>1000) THEN BEGIN
OUTSTR("ILLEGAL STEP SPECIFICATION"&CRLF);
GOTO NXT;
END;
STEPS←1000/STEPS;
DPYCLR;
QUICK_CODE -8304460352; END; ⊃ DPYPOS -1100, makes PP invisible;
POG←GETPOG;
DPYSET(DISPLY);
TT←0.0;
CT←0;
K←-1;
CASE CS1 OF BEGIN
COM1 ← "JT " & CVS(JT) & MESSG;
COM1←MESSG;
END;
COM2←"SEG. & END TIME: ";
FOR I←SEG1 STEP 1 UNTIL SEG2 DO BEGIN
TTOTAL←TIME[I];
CT←CT+TIME[I];
COM2←COM2&CVS(I)&":"&CVS(CT)&" / ";
WHILE TT≤TTOTAL DO BEGIN
PT←TT/TTOTAL;
K←K+1;
CASE CS1 OF BEGIN
ANGLE[K]←((((((COEF[I,JT,6]*PT)
+COEF[I,JT,5])*PT
+COEF[I,JT,4])*PT+COEF[I,JT,3])*PT
+COEF[I,JT,2])*PT+COEF[I,JT,1])*10.0;
BEGIN
FOR JT←1 STEP 1 UNTIL 6 DO
JTS[JT]←((((((COEF[I,JT,6]*PT)
+COEF[I,JT,5])*PT
+COEF[I,JT,4])*PT+COEF[I,JT,3])*PT
+COEF[I,JT,2])*PT+COEF[I,JT,1]);
UPDATE(TRANS,JTS);
UNSTRUCT(TRANS,EANGLE);
X[K]←EANGLE[1]*10.0;
Y[K]←EANGLE[2]*10.0;
Z[K]←EANGLE[3]*10.0;
O[K]←EANGLE[4]*10.0;
A[K]←EANGLE[5]*10.0;
T[K]←EANGLE[6]*10.0;
END;
END;
TT←TT+STEPS;
END;
TT←TT-TTOTAL;
END;
CASE CS2 OF BEGIN
ARRGRF(ANGLE,0,K,-400,-400,800,800,CVS(STEPS)&
" MSEC/INCREMENT","DEGREES*10");
BEGIN
ARRGRF(X,0,K,-400,150,800,250,CVS(STEPS)&
" MSEC/INCREMENT","X POSITION IN 1/10 INCH");
ARRGRF(Y,0,K,-400,-150,800,250,CVS(STEPS)&
" MSEC/INCREMENT","Y POSITION IN 1/10 INCH");
ARRGRF(Z,0,K,-400,-450,800,250,CVS(STEPS)&
" MSEC/INCREMENT","Z POSITION IN 1/10 INCH");
END;
BEGIN
ARRGRF(O,0,K,-400,150,800,250,CVS(STEPS)&
" MSEC/INCREMENT","O VECTOR IN DEG*10");
ARRGRF(A,0,K,-400,-150,800,250,CVS(STEPS)&
" MSEC/INCREMENT","A VECTOR IN DEG*10");
ARRGRF(T,0,K,-400,-450,800,250,CVS(STEPS)&
" MSEC/INCREMENT","T VECTOR IN DEG*10");
END;
END;
DPYBIG(2);
AIVECT(-400,475);
DPYSST(COM1);
AIVECT(-400,450);
DPYSST(COM2);
AIVECT(-400,425);
DPYSST(COM3);
DPYOUT(POG);
INCHWL;
DPYCLR;
DANS←RELD;
GOTO NXT;
END;
⊃ IF P COMMAND, WRITE OUT A DISK FILE CONTAINING THE DISPLAY BUFFER.;
NXTFUN: IF EQU(DISCM,"P") THEN BEGIN
STRING FILNAM;
INTEGER FLG,CHN;
CHN←14;
OPEN(CHN,"DSK",8,0,3,0,0,0);
DO BEGIN
OUTSTR("PLOT FILE = ");
FILNAM←INCHWL;
ENTER(CHN,FILNAM&".PLT",FLG);
END UNTIL ¬FLG;
ARRYOUT(CHN,DISPLY[1],DISPLY[2]);
CLOSE(CHN);
RELEASE(CHN);
GOTO NXT;
END;
⊃ MUST BE EITHER "E" OF AN ILLEGAL INSTRUCTION;
IF ¬EQU(DISCM,"E") THEN OUTSTR("ILLEGAL INSTRUCTION"&CRLF);
NXT: END "DB" UNTIL EQU(DISCM,"E");
END "DISPLAY";
⊃ MAIN LOOP;
WHILE TRUE DO DISPLAY(" ")
END